#include "socket_can.h"

#include "utils/Log.h"
#include "can/context.h"

namespace socket_can {

SocketCAN::~SocketCAN() {
	stop();
}

bool SocketCAN::startup() {
	if (bitrate_ <= 0) {
		bitrate_ = 100000;
	}
	LOGD("Start UP Can socket bitrate:%d \n", bitrate_);
	const char* canSetBitrate =
			"/res/bin/ip link set can0 type can bitrate %d triple-sampling on";
	char buff[256];
	sprintf(buff, canSetBitrate, bitrate_);
	LOGD("set can bitrate:%s \n", buff);
	system(buff);
	const char* canup_ = "/res/bin/ip link set can0 up";
	system(canup_);
	init();
	run("canreader");
	return true;
}

bool SocketCAN::stop() {
	requestExitAndWait();
	if (close(socket_) < 0) {
		LOGD("Closing: ");
		LOGD("Error: %d", errno);
	}
	connected_ = false;
	const char* candwon_ = "/res/bin/ip link set can0 down";
	system(candwon_);
	return true;
}

SocketCAN::SocketCAN(const char* ifname, int bitrate, int timeout) :
		ifname_(ifname), socket_(-1), connected_(false), timeout_(timeout), bitrate_(
				bitrate)

{
	LOGD("Socket Can Open \n");
	CBCanRead_ = NULL;
}

bool SocketCAN::threadLoop() {
	while (connected_) {
		can_frame canframe_;
		int ret = read(&canframe_);
		if (ret) {
//			if(CBCanRead_){
//				(CBCanRead_)(canframe_);
//			}
//			LOGD("read!!!!!\n");
//			canframe_.can_id = canframe_.can_id&0x7FFFFFFF;
//			notifyCanProtocolDataUpdate(canframe_);
			can::parseProtocol(canframe_);
		} else {
//			LOGD("error read!!!!!\n");
		}
		return true;
	}
	return false;
}

SocketCAN* SocketCAN::getInstance() {
	static SocketCAN* ptr = new SocketCAN("can0", 500000, 10);
	return ptr;
}

void SocketCAN::setFilter(can_filter* filterArray, int filterSize) {
	setsockopt(socket_, SOL_CAN_RAW, CAN_RAW_FILTER, filterArray,
			sizeof(can_filter) * filterSize);
}

void SocketCAN::init() {
	if ((socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
		LOGE("Error while opening socket");
		return;
	}

	struct ifreq ifr { };
	strcpy(ifr.ifr_name, ifname_);
	ioctl(socket_, SIOCGIFINDEX, &ifr);

	struct sockaddr_can addr { };
	addr.can_family = AF_CAN;
	addr.can_ifindex = ifr.ifr_ifindex;

	LOGD("%s at index %d\n", ifname_, ifr.ifr_ifindex);

	if (bind(socket_, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		LOGE("Error in socket bind");
		return;
	}

	int error = 0;
	socklen_t len = sizeof(error);
	int retval = getsockopt(socket_, SOL_SOCKET, SO_ERROR, &error, &len);
	if (retval != 0) {
		/* there was a problem getting the error code */
		LOGE("Error getting socket error code: %s\n", strerror(retval));
		return;
	}

	if (error != 0) {
		/* socket has a non zero error status */
		LOGE("--%d-- --%s-- Socket error: %s\n", __LINE__, __FILE__,strerror(error));
		return;
	}

	struct timeval timeout { };
	timeout.tv_sec = (timeout_ / 1000);
	timeout.tv_usec = (timeout_ % 1000) * 1000;

	if (setsockopt(socket_, SOL_SOCKET, SO_RCVTIMEO, (char *) &timeout,
			sizeof(timeout)) < 0) {
		LOGE("Setting timeout failed");
	}

	connected_ = true;
}

bool SocketCAN::is_connected() {
	return connected_;
}

bool SocketCAN::write(uint32_t can_id, uint8_t dlc, uint8_t * data) {
	struct can_frame frame { };

	frame.can_id = (can_id > 0x7FF) ? (can_id | CAN_EFF_FLAG) : can_id;
//  frame.can_id = can_id | CAN_EFF_FLAG;
	frame.can_dlc = dlc;
	memcpy(frame.data, data, dlc * sizeof(uint8_t));

	auto num_bytes = ::write(socket_, &frame, sizeof(struct can_frame));
//	LOGE("can tx : ID=0x%x DLC=%d \n", can_id, dlc);

	return num_bytes > 0;
}

void SocketCAN::setCanReadCb(OnCanReadData cb) {
	CBCanRead_ = cb;
}

bool SocketCAN::read(can_frame* canFrame) {
	int num_bytes = ::read(socket_, canFrame, sizeof(struct can_frame));
	if (num_bytes != sizeof(struct can_frame)) {
		if (num_bytes > 0) {
			LOGE("read out num error:%d \n", num_bytes);
		}
		return 0;
	}
	return true;
}

}

